/*
Simplo 0.26
 2012 06 13
 Copyrigh(C) 2012 by Francesco Ferrara
 
 DONE
 - Flight is ok with a QUADX and HEXAX
 - channel D5 D6 or A0 A1 work with 500hz or 50hz
 - compatibile with v19 MultiWii Config
 - Working ITG3200, BMA180, HMC5843, HMC5883L
 - PPM in D2 8 channels max
 - zeroing gyro same as multiwii thr down rud left ele down
 - zeroing acc same as multiwii thr up rud left ele down via GUI
 - zeroing mag thr down rud left ele up and also via GUI
 - expo is currently used for the stable factor to return to home position
 
 
 SPECIAL
 - ORIENTATION is always X front Y left Z up, right hand rule for rotation
 - sending a D command, all info about sensors are printed on the arduino terminal 
 - sending a R command, EEPROM will be reset
 - Using AHRS Madwick to know the orientation 
 - FULL Floating point operation
 - clean code
 - Proportional value are 100x sensible respect to Multiwii 
 - introduced def.h that check the defines
 - GYRO_ORIENTATION
 - ACC_ORIENTATION
 - Corrected the calibration with MAG 
 - stable mode using the data from ahrs
 
 TODO
 - using mag for carefree
 - remove any reference to MULTIWII
 - change the rising edge on the RX routine losing last PPMSUM channel
 - build a new GUI with corrected value
 - a stronger FAILSAFE (invalidate all block if some channel are wrong)
 
 
 CHANGELOG
 - 0.26
 corrected value returned to GUI 2.0 for angle calculated
 - 0.25
 compatibility to GUI 2.0
 supported acc&gyro MPU6050
 - 0.24
 BARO BMP085 added with a state machine to a very fast approach and mobile media output is only the pressure not altitude. Temperature are readed every 0.5 seconds
 - 0.23
 added i2c error for acc & mag
 - 0.22 
 added HEXAP HEXA plus 
 added autoreset i2c and errorCount errorCode in gyro
 corrected bug for default Derivative too big
 
 - 0.21 
 changed DEFINE constant to static const locally to the class
 
 - 0.2 
 corrected some bug relative to setup & EEPROM
 new calibration routing for HMC5883L
 stable mode via new channel5 
 
 */

// Global var
extern void loadAll();
extern void saveAll();
int cycle;
boolean debug;

// Order is important 
#include <EEPROM.h>
#include "ahrs.h"
#include "parameter.h"
#include "fastwire.h"
#include "setup.h"
#include "def.h"
#include "disk.h"
#include "pid.h"
#include "esc.h"
#include "rx.h"
#include "led.h"
#include "gyro.h"
#include "acc.h"
#include "mag.h"
#include "mixer.h"
#include "servo.h"
#include "baro.h"

#include "gui.h"


/* EEPROM Function */
/*******************************************************/

void saveAll()
{
  para.setVersion();
  gyro.save();
  pRoll.save();
  pPitch.save();
  pYaw.save();
  mixer.save();
  acc.save();
  mag.save();
}

void loadAll()
{
  if (para.checkVersion())
  {
    gyro.load();
    pRoll.load();
    pPitch.load();
    pYaw.load();
    mixer.load();
    acc.load();
    mag.load();
  }
  else
    saveAll();

}

/*******************************************************/

void setup()
{
  byte ret;

  fw.setup(400,PULLUP);   
  Serial.begin(115200);

  ret=gyro.setup();
#ifdef GYRO_PRESENT
  if (ret!=0) { 
    Serial.print("ERROR: Gyro not initialized."); 
    while(1); 
  }
#endif

  ret=acc.setup();
#ifdef ACC_PRESENT

  if (ret!=0) { 
    Serial.print("ERROR: Acc not initialized."); 
    while(1); 
  }
#endif

  ret=mag.setup();
#ifdef MAG_PRESENT
  if (ret!=0) {
    Serial.print("ERROR: Mag not initialized."); 
    while(1); 
  }
#endif

  led.setup();
  rx.setup();
  mixer.setup();
  esc.setup();
  pRoll.setup (0.7, 0.150, 0);
  pPitch.setup(0.7, 0.150, 0);
  pYaw.setup  (0.6, 0.100, 0);
  ahrs.setup();
  baro.setup();
#ifdef SERVO
  servo.setup();
#endif
  loadAll();

  //Serial.println("RESET");

}

/********************************************************************************************************/

unsigned long curTime,lastTime;
int rate5; // every 5 hz this value goes to 0
int rate50; // every 50 hz this value goes to 0

void loop()
{
  byte ret;
  curTime=micros();
  cycle=curTime-lastTime;

  if (cycle>=CYCLEMIN)
  {
    lastTime=curTime;

    if (rate5++>=0.200*CYCLERATE) // in seconds
      rate5=0;

    if (rate50++>=0.020*CYCLERATE) // in seconds
      rate50=0;

#ifdef GYRO_PRESENT
    gyro.update();

#endif
#ifdef BARO_PRESENT
    baro.update();
#endif
    if (rate50==0) // 50hz
    {

      rx.update();
#ifdef ACC_PRESENT
      acc.update();
#endif
#ifdef MAG_PRESENT      
      mag.update();
#endif

#if defined(ACC_PRESENT) 
#if defined(MAG_PRESENT)
      ahrs.updateMARG(gyro.r,gyro.p,gyro.y, acc.x,acc.y,acc.z, mag.x,mag.y,mag.z);
#else
      ahrs.updateIMU(gyro.r,gyro.p,gyro.y, acc.x,acc.y,acc.z);
#endif  
#endif
    }
    led.update();
    mixer.update();
    esc.update();
#ifdef SERVO
    servo.update();
#endif
    gui();


    if (debug)
    {
      if (rate5==0) 
      {
        /*
        for(byte i=0;i<MAXCHANIN;i++)
         {
         Serial.print(rx.channelIn[i]);
         Serial.print(";");
         }
         Serial.print("\t");
         
         for(byte i=0;i<MAXCHANOUT;i++)
         {
         Serial.print(esc.ichannelOut[i]);
         Serial.print(";");
         }
         Serial.print("\t");
         
         Serial.print(gyro.ri); 
         Serial.print("\t");
         Serial.print(gyro.pi); 
         Serial.print("\t");
         Serial.print(gyro.yi); 
         Serial.print(";\t");
         
         Serial.print(gyro.r0); 
         Serial.print("\t");
         Serial.print(gyro.p0); 
         Serial.print("\t");
         Serial.print(gyro.y0); 
         Serial.print(";\t");
         */
        Serial.print(gyro.r); 
        Serial.print(";");
        Serial.print(gyro.p); 
        Serial.print(";");
        Serial.print(gyro.y); 
        Serial.print(";\t");


        Serial.print(acc.x); 
        Serial.print(";");
        Serial.print(acc.y); 
        Serial.print(";");
        Serial.print(acc.z); 
        Serial.print(";\t");

        Serial.print(mag.x); 
        Serial.print(";");
        Serial.print(mag.y); 
        Serial.print(";");
        Serial.print(mag.z); 
        Serial.print(";\t");

        Serial.println();


      }
    }  
  }
}













































